#include "cartographer/rcm/particle_filter/map_2d.h"

namespace cartographer
{
    int Map2d::Read(const std::string &filename)
    {
        std::ifstream filestr(filename.c_str(), std::ifstream::in);
        if (!filestr)
            return -1;
        std::string line_str;
        while (getline(filestr, line_str))
        {
            std::istringstream iss(line_str);
            Landmark lm = std::make_shared<Landmark>();
            Eigen::Vector3d vector(0, 0, 0);
            iss >> vector(0);
            iss >> vector(1);
            iss >> lm->id;
            lm->pose = std::make_shared<Pose2d>(vector);
            landmarks_.push_back(lm);
        }
        return 1;
    }

    const Pose::Ptr Map2d::SearchNearPose(const Pose::Ptr &pose)
    {
        Pose::Ptr near;
        double distMin = std::numeric_limits<double>::max();
        for (const auto &lm : landmarks_)
        {
            double distance = lm->pose->Distance(pose);
            if (distance < distMin)
            {
                distMin = distance;
                near = lm->pose;
            }
        }
        return near;
    }
} // namespace cartographer